package ece496.gps;

import java.util.ArrayList;
import java.util.Iterator;

import android.util.Log;

public class FeatureListObject {
	ArrayList<FeatureObject> mapFeatureWALL;
	ArrayList<FeatureObject> mapFeatureDOOR;
	ArrayList<FeatureObject> mapFeatureSTAIRS;
	ArrayList<FeatureObject> mapFeatureELV;
	ArrayList<ArrayList<FeatureObject>> mapFeatureList;
	
	FeatureListObject() {
		mapFeatureWALL = new ArrayList<FeatureObject>();
		mapFeatureDOOR = new ArrayList<FeatureObject>();
		mapFeatureSTAIRS = new ArrayList<FeatureObject>();
		mapFeatureELV = new ArrayList<FeatureObject>();
		
		mapFeatureList = new ArrayList<ArrayList<FeatureObject>>();
		mapFeatureList.add(mapFeatureWALL);
		mapFeatureList.add(mapFeatureDOOR);
		mapFeatureList.add(mapFeatureSTAIRS);
		mapFeatureList.add(mapFeatureELV);
	}
	
	void addFeature(String type, FeatureObject tempFeatureObj) {
		if (type.equals("WALL")) {
			mapFeatureWALL.add(tempFeatureObj);
		}
		else if (type.equals("DOOR")) {
			mapFeatureDOOR.add(tempFeatureObj);
		}
		else if (type.equals("STAIRS")) {
			mapFeatureSTAIRS.add(tempFeatureObj);
		}
		else if (type.equals("ELEVATOR")) {
			mapFeatureELV.add(tempFeatureObj);
		}
	}
	
	// Returns closest feature by searching through and finding closest x and y
	// Returns null if no features stored
	FeatureObject findClosestFeature(int x, int y) {
		FeatureObject foundFeature = null;
		FeatureObject tempFeature = null;
		double distance = 1000000;
		double closestDistance = 1000000;
		
		Log.i("findClosestFeature", "Created variables for findClosestFeature");
		// Iterate through list of features
		for (Iterator<ArrayList<FeatureObject>> arListIT = mapFeatureList.iterator(); arListIT.hasNext();) {
			// Iterate through features to find closest object
			Log.i("findClosestFeature", "Created iterator for feature list");

			ArrayList<FeatureObject> tempArrayList = arListIT.next();
			for (Iterator<FeatureObject> featureIT = tempArrayList.iterator(); featureIT.hasNext();) {
				tempFeature = featureIT.next();
				Log.i("findClosestFeature", "Created iterator for FeatureObject of type "+tempFeature.featureType);
				
				// Wall detection
				// Uses absolute distance from horizontal or vertical wall
				if (tempFeature.featureType.equals("WALL")) {
					// Walls will be either vertical or horizontal
					if (tempFeature.xStart == tempFeature.xEnd) {
						for (int yTemp = tempFeature.yStart; yTemp <= tempFeature.yEnd; yTemp++) {
							distance = Math.hypot((tempFeature.xStart - x), (yTemp - y));
							if (distance < closestDistance) {
								closestDistance = distance;
								foundFeature = tempFeature;
							}
						}
					}
					else if (tempFeature.yStart == tempFeature.yEnd) {
						for (int xTemp = tempFeature.xStart; xTemp <= tempFeature.xEnd; xTemp++) {
							distance = Math.hypot((xTemp - x), (tempFeature.yStart - y));
							if (distance < closestDistance) {
								closestDistance = distance;
								foundFeature = tempFeature;
							}
						}
					}
					Log.i("findClosestFeature", "Distance to object of type "+tempFeature.featureType+" found as "+distance);
				}
				// Door/Stairs/Elevator detection
				// Uses hypot to find the closest feature
				else {
					distance = Math.hypot((tempFeature.xStart - x), (tempFeature.yStart - y));
					Log.i("findClosestFeature", "Distance to object of type "+tempFeature.featureType+" found as "+distance);

					// Doors/Stairs/Elevators should take preference
					// Even if wall is found, if distance to D/S/E is equidistant then take preference to D/S/E 
					if (distance <= closestDistance) {
						closestDistance = distance;
						foundFeature = tempFeature;
					}
				}
			}
		}
		
		return foundFeature;
	}
   
   FeatureObject findClosestWallIntersect(int x1, int x2, int y1, int y2) {
		FeatureObject tempFeature = null;
		FeatureObject givenCoordinates = new FeatureObject();
		FeatureObject intersectCoord = null;
		
		givenCoordinates.xStart = Math.min(x1,x2);
		givenCoordinates.xEnd = Math.max(x1,x2);
		givenCoordinates.yStart = Math.min(y1,y2);
		givenCoordinates.yEnd = Math.max(y1,y2);
		
		Log.i("findClosestWallIntersect", "Finished initializing and setting up variables\n");
		
		// Iterate through all wall objects
        for (Iterator<FeatureObject> featureIT = mapFeatureWALL.iterator(); featureIT.hasNext();) {
			tempFeature = featureIT.next();
			
			Log.i("findClosestWallIntersect", "Calling intersect function with coordinates x1:"+x1+" x2:"+x2+" y1:"+y1+" y2:"+y2+"\n");					
			intersectCoord = findIntersect(givenCoordinates, tempFeature);
			if (intersectCoord == null) {
				Log.i("findClosestWallIntersect", "Intersect function returned null; no intersect between points\n");
			}
        }
        
        return intersectCoord;
	}
	
	FeatureObject findIntersect (FeatureObject givenCoordinates, FeatureObject tempFeature) {
		FeatureObject intersectPoints = null;
		// Find y = mx+b first
		float w = givenCoordinates.xEnd - givenCoordinates.xStart;
		float h = givenCoordinates.yEnd - givenCoordinates.yStart;
		float b = 0;
		
		Log.i("findIntersect", "Rise and run are h:"+h+" w:"+w+"\n");
		if (w != 0)
			b = givenCoordinates.yStart - h/w*givenCoordinates.xStart;
		else
			Log.i("findIntersect", "w = 0, divide by 0\n");
		
		Log.i("findIntersect", "Values are m:"+h/w+" b:"+b+"\n");
		
		int intersectX = -1, intersectY = -1;
		if (tempFeature.xStart == tempFeature.xEnd) {
			// find by y = mx+b
			// if h = 0 means horizontal line, check if intersect with vertical wall
			if (h == 0) {
				intersectX = tempFeature.xStart;
				intersectY = givenCoordinates.yStart;
				Log.i("findIntersect", "Intersection of HOR line with VER wall\n");
			}
			// Make sure that w != 0 because that means parallel lines (VER wall and VER line)
			else if (w != 0) {
				intersectX = tempFeature.xStart;
				intersectY = Math.round(h/w * tempFeature.xStart + b);
				Log.i("findIntersect", "Intersection of line with VER wall\n");
			}
			Log.i("findIntersect", "VER Wall; intersectX: "+intersectX+" and intersectY:"+intersectY+"\n");
		}
		else if (tempFeature.yStart == tempFeature.yEnd) {
			// x = (y-b)/m
			// if w = 0 means vertical line, check if intersect with horizontal wall
			if (w == 0) {
				intersectX = givenCoordinates.xStart;
				intersectY = tempFeature.yStart;
				Log.i("findIntersect", "Intersection of VER line with HOR wall\n");
			}
			// if h = 0 means HOR line, cannot intersect with HOR wall
			else if (h != 0) {
				intersectX = Math.round((tempFeature.yStart - b)*w/h);
				intersectY = tempFeature.yStart;
				Log.i("findIntersect", "Non-zero slope line intersect with horizontal wall\n");
			}
				
			Log.i("findIntersect", "Horizontal Wall; intersectX: "+intersectX+" and intersectY:"+intersectY+"\n");
		}
		else {
			Log.i("findIntersect", "Non vertical/horizontal wall; unexpected wall type\n");
		}
		
		// Check if the intersection lies in the line of the two points
		if (givenCoordinates.xStart <= intersectX && intersectX <= givenCoordinates.xEnd &&
			givenCoordinates.yStart <= intersectY && intersectY <= givenCoordinates.yEnd) {
			
			intersectPoints = new FeatureObject();
			intersectPoints.xStart = intersectX;
			intersectPoints.yStart = intersectY;
		}
        // return FeatureObject with intersect coordinates stored in xStart and yStart
        return intersectPoints;
	}
}
